/**
 * @file Judge.cpp
 * @author enemy1205 (enemy1205@qq.com)
 * @brief 决策类实现
 * @date 2021-10-03
 */
#include "Judge.h"

Judge::Judge() {
    pTrackingTarget = nullptr;
    score = 10000.f;
    hasTarget= false;
}

void ArmorJudge::Judging(std::vector<tracker_ptr> &trackers, const bool &hasNoFeatures) {
    if (hasNoFeatures) {
        hasTarget = false;
        judgeData[0] = 72;//无目标
        judgeData[1] = 64;//禁止射击
        judgeData[2] = 0;//射频0
    } else {
        for (const auto &tracker: trackers) {
            float temp_score = (tracker->getPnp().distance/100 * 0.8 + tracker->getGoalFeature()->getMatchError() * 0.1 +
                                abs(tracker->getGoalFeature()->getAngle() - 0) * 0.1)/(tracker->getPnp().distance/100 + tracker->getGoalFeature()->getMatchError()+
                                abs(tracker->getGoalFeature()->getAngle()));
//                                std::cout << "score"<<temp_score << std::endl;
            if (temp_score < this->score) {
                this->score = temp_score;
                this->pTrackingTarget = tracker;
            }
        }
        hasTarget = true;
        judgeData[0] = 37;//有目标
        judgeData[1] = 76;//允许射击
        judgeData[2] = 30;//射频30
    }
}

void RuneJudge::Judging(std::vector<tracker_ptr> &trackers,const bool &hasFeatures) {

}